//
// File: observed_tar_input_v2.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 27-Dec-2024 11:47:56
//

// Include Files
#include "observed_tar_input_v2.h"
#include <cmath>

// Function Definitions
//
// Arguments    : double X
//                double Y
//                double car_angle
//                double tarX
//                double tarY
//                double *dg
//                double *sig
// Return Type  : void
//
void observed_tar_input_v2(double X, double Y, double car_angle, double tarX,
                           double tarY, double *dg, double *sig)
{
  double absxk;
  double d;
  double scale;
  double sig_dot;
  double t;
  // dg
  scale = 3.3121686421112381E-170;
  // sigd
  d = tarX - X;
  absxk = std::abs(d);
  if (absxk > 3.3121686421112381E-170) {
    *dg = 1.0;
    scale = absxk;
  } else {
    t = absxk / 3.3121686421112381E-170;
    *dg = t * t;
  }
  sig_dot = d;
  d = tarY - Y;
  absxk = std::abs(d);
  if (absxk > scale) {
    t = scale / absxk;
    *dg = *dg * t * t + 1.0;
    scale = absxk;
  } else {
    t = absxk / scale;
    *dg += t * t;
  }
  *dg = scale * std::sqrt(*dg);
  *sig = std::acos(sig_dot / *dg);
  if (d < 0.0) {
    *sig = -*sig - car_angle;
  } else {
    *sig -= car_angle;
  }
}

//
// File trailer for observed_tar_input_v2.cpp
//
// [EOF]
//
